#include "WmmxImageLib32.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

struct _ErrorColor{
    float dR;
    float dG;
    float dB;
};

inline int Power2m1(int exp)
{
    int result = 1;
    for (int i=0;i<exp;i++) {
        result =result<<1;
    }
    return result-1;
}

inline long getC8Color(const long rColor){
    return rColor*(((1<<16)*255+30)/31) >>16;
}

inline long getBestColor(const float wantColor, int precision){
    float result=float(wantColor*((float(precision))/255));
    if (result<=0)
        return 0;
    else if (result>=precision)
        return precision;
    else
        return (long)result;
}

inline void BlendPixel2(IMAGE *srcImg, int x, int y, IMAGE *img, unsigned char srcAlpha)
{
    PIXEL* Dst = (PIXEL*)img->pData;
    PIXEL* Src = (PIXEL*)srcImg->pData;
    ((char*&)Dst)+=img->nPitch*y;
    ((char*&)Src)+=srcImg->nPitch*y;
    Dst+=x;
    Src+=x;

    unsigned char alpha = (Dst->A * srcAlpha)>>8;

    Dst->A = ((Src->A*srcAlpha)>>8) + ((Dst->A * (255-alpha))>>8) > 255 ? 255 : ((Src->A*srcAlpha)>>8) + ((Dst->A * (255-alpha))>>8);
    Dst->R = ((Src->R*srcAlpha)>>8) + ((Dst->R * (255-alpha))>>8) > 255 ? 255 : ((Src->R*srcAlpha)>>8) + ((Dst->R * (255-alpha))>>8);
    Dst->G = ((Src->G*srcAlpha)>>8) + ((Dst->G * (255-alpha))>>8) > 255 ? 255 : ((Src->G*srcAlpha)>>8) + ((Dst->G * (255-alpha))>>8);
    Dst->B = ((Src->B*srcAlpha)>>8) + ((Dst->B * (255-alpha))>>8) > 255 ? 255 : ((Src->B*srcAlpha)>>8) + ((Dst->B * (255-alpha))>>8);

}


inline PIXEL PixelAlpha888_PreMul(PIXEL Src, PIXEL Dst, unsigned char srcAlpha)
{
    PIXEL Result;

    unsigned char alpha = (Src.A * srcAlpha)>>8;

    Result.A = ((Src.A*srcAlpha)>>8) + ((Dst.A * (255-alpha))>>8) > 255 ? 255 : ((Src.A*srcAlpha)>>8) + ((Dst.A * (255-alpha))>>8);
    Result.R = ((Src.R*srcAlpha)>>8) + ((Dst.R * (255-alpha))>>8) > 255 ? 255 : ((Src.R*srcAlpha)>>8) + ((Dst.R * (255-alpha))>>8);
    Result.G = ((Src.G*srcAlpha)>>8) + ((Dst.G * (255-alpha))>>8) > 255 ? 255 : ((Src.G*srcAlpha)>>8) + ((Dst.G * (255-alpha))>>8);
    Result.B = ((Src.B*srcAlpha)>>8) + ((Dst.B * (255-alpha))>>8) > 255 ? 255 : ((Src.B*srcAlpha)>>8) + ((Dst.B * (255-alpha))>>8);

    return Result;
//    return ( (unsigned char*)((unsigned char*)pic.paData+pic.naPitch*y) )[x];
}

inline PIXEL PixelAlpha_PreMul(PIXEL Src, PIXEL Dst)
{
    PIXEL Result;

    Result.A = Src.A + ((Dst.A * (255-Src.A))>>8);// > 255 ? 255 : ((Src.A*srcAlpha)>>8) + ((Dst.A * (255-alpha))>>8);
    Result.R = Src.R + ((Dst.R * (255-Src.A))>>8);// > 255 ? 255 : ((Src.R*srcAlpha)>>8) + ((Dst.R * (255-alpha))>>8);
    Result.G = Src.G + ((Dst.G * (255-Src.A))>>8);// > 255 ? 255 : ((Src.G*srcAlpha)>>8) + ((Dst.G * (255-alpha))>>8);
    Result.B = Src.B + ((Dst.B * (255-Src.A))>>8);// > 255 ? 255 : ((Src.B*srcAlpha)>>8) + ((Dst.B * (255-alpha))>>8);

    return Result;
//    return ( (unsigned char*)((unsigned char*)pic.paData+pic.naPitch*y) )[x];
}

void BresenhamDrawLine(int xs,int ys,int xe,int ye, int color, IMAGE * img)
{
	int dx=xe-xs;
	int dy=ye-ys;
	int xinc,yinc,i,x,y;

	if(dx>0)
		xinc=1;
	else
		xinc=-1;

	if(dy>0)
		yinc=1;
	else
		yinc=-1;

	dx=abs(dx);
	dy=abs(dy);
	x=xs;
	y=ys;
	i=0;

	if(dx==0&&dy==0)
		SetPixel(x, y, color, img);
	else if(dx==0)
	{
		for(i=0;i<dy;i++)
		{
			SetPixel(x, y, color, img);
			y+=yinc;
		}
	}
	else if(dy==0)
	{
		for(i=0;i<dx;i++)
		{
			SetPixel(x, y, color, img);
			x+=xinc;
		}
	}
	else if(dx>dy)
	{
		int p=2*dy-dx;
		int inc1=2*dy,inc2=2*(dy-dx);
		for(i=0;i<dx;i++)
		{
			SetPixel(x, y, color, img);
			x+=xinc;
			if(p<0)
				p+=inc1;
			else
			{
				y+=yinc;
				p+=inc2;
			}
		}
	}
	else
	{
		int p=2*dx-dy;
		int inc1=2*dx,inc2=2*(dx-dy);
		for(i=0;i<dy;i++)
		{
			SetPixel(x, y, color, img);
			y+=yinc;
			if(p<0)
				p+=inc1;
			else
			{
				x+=xinc;
				p+=inc2;
			}
		}
	}
	return;
}

void GradientFillColor(int* gradientTable, int x0, int y0, int vx, int vy, IMAGE *img)
{

    int len = vx*vx+vy*vy;

    _GRADFILL gfill;
    gfill.pDst = img->pData;
    gfill.nPitch = img->nPitch;
    gfill.incx_16 = int(((long long int)(vx)<<24)/len);
    gfill.incy_16 = int(((long long int)(vy)<<24)/len);
    gfill.x0 = x0;
    gfill.y0 = y0;
    gfill.gradientTable = gradientTable;
    gfill.width = img->nWidth;

    GradientFill(gfill);
}

void GradientFillAlpha(unsigned char* gradientTable, int x0, int y0, int vx, int vy, IMAGE *img)
{

}

void ScanLeftAAEdge(POSITION ps, POSITION pe, ScanLineList * scanlinelist, int startY, int height, IMAGE * srcImg, IMAGE * dstImg)
{
	int dx=pe.x-ps.x;
	int dy=pe.y-ps.y;
	int xinc,yinc,i,x,y;

	if (dx>0)
		xinc=1;
	else
		xinc=-1;

	if(dy>0)
		yinc=1;
	else
		yinc=-1;

	dx=abs(dx);
	dy=abs(dy);
	x=pe.x;
	y=pe.y;
	scanlinelist[y-startY].Left.x = x;
	scanlinelist[y-startY].Left.y = y;
    BlendPixel2(srcImg, x, y, dstImg, 255);
	x=ps.x;
	y=ps.y;
	scanlinelist[y-startY].Left.x = x;
	scanlinelist[y-startY].Left.y = y;
    BlendPixel2(srcImg, x, y, dstImg, 255);

	i=0;

	if(dx==0&&dy==0)
		BlendPixel2(srcImg, x, y, dstImg, 255);
	else if(dx==0)
	{
		for(i=0;i<dy;i++)
		{
			BlendPixel2(srcImg, x, y, dstImg, 255);
            scanlinelist[y-startY].Left.x = x;
            scanlinelist[y-startY].Left.y = y;
			y+=yinc;
		}
	}
	else if(dy==0)
	{
		for(i=0;i<dx;i++)
		{
			BlendPixel2(srcImg, x, y, dstImg, 255);
			x+=xinc;
		}
	}
	else if(dx>dy)
	{
	    if (xinc>0) {
            _AAEDGEINFO aaedge;
    	    aaedge.s16 = y<<16;
            aaedge.d16 = (dy<<16)/dx;
            aaedge.startP = x;
            aaedge.deltaD = dx;
            aaedge.pDst = dstImg->pData;
            aaedge.pSrc = srcImg->pData;
            aaedge.nPitch = dstImg->nPitch;
            aaedge.inc = xinc;
            aaedge.scanlinelist = scanlinelist;
            aaedge.startY = startY;
            DrawXMajorLPEdge(aaedge);
		}
		else {
            _AAEDGEINFO aaedge;
    	    aaedge.s16 = y<<16;
            aaedge.d16 = (dy<<16)/dx;
            aaedge.startP = x;
            aaedge.deltaD = dx;
            aaedge.pDst = dstImg->pData;
            aaedge.pSrc = srcImg->pData;
            aaedge.nPitch = dstImg->nPitch;
            aaedge.inc = xinc;
            aaedge.scanlinelist = scanlinelist;
            aaedge.startY = startY;
            DrawXMajorLNEdge(aaedge);
	    }
    }
	else
	{
        _AAEDGEINFO aaedge;
	    aaedge.s16 = x<<16;
        aaedge.d16 = (dx<<16)/dy;
        if (xinc<0) {
           aaedge.d16=-aaedge.d16;
        }
        aaedge.startP = y;
        aaedge.deltaD = dy;
        aaedge.pDst = dstImg->pData;
        aaedge.pSrc = srcImg->pData;
        aaedge.nPitch = dstImg->nPitch;
        aaedge.inc = yinc;
        aaedge.scanlinelist = scanlinelist;
        aaedge.startY = startY;
        DrawYMajorLEdge(aaedge);
	}
	return;
}

void ScanRightAAEdge(POSITION ps, POSITION pe, ScanLineList * scanlinelist, int startY, int height, IMAGE * srcImg, IMAGE * dstImg)
{
	int dx=pe.x-ps.x;
	int dy=pe.y-ps.y;
	int xinc,yinc,i,x,y;

	if (dx>0)
		xinc=1;
	else
		xinc=-1;

	if(dy>0)
		yinc=1;
	else
		yinc=-1;

	dx=abs(dx);
	dy=abs(dy);

	x=pe.x;
	y=pe.y;
	scanlinelist[y-startY].Right.x = x;
	scanlinelist[y-startY].Right.y = y;
//    SetPixel(x, y, color, img);

	x=ps.x;
	y=ps.y;
	scanlinelist[y-startY].Right.x = x;
	scanlinelist[y-startY].Right.y = y;
//    SetPixel(x, y, color, img);

	i=0;

	if(dx==0&&dy==0)
		BlendPixel2(srcImg, x, y, dstImg, 255);
	else if(dx==0)
	{
		for(i=0;i<dy;i++)
		{
			BlendPixel2(srcImg, x, y, dstImg, 255);
            scanlinelist[y-startY].Right.x = x;
            scanlinelist[y-startY].Right.y = y;
			y+=yinc;
		}
	}
	else if(dy==0)
	{
		for(i=0;i<dx;i++)
		{
			BlendPixel2(srcImg, x, y, dstImg, 255);
			x+=xinc;
		}
	}
	else if(dx>dy)
	{
	    if (xinc>0) {
            _AAEDGEINFO aaedge;
    	    aaedge.s16 = y<<16;
            aaedge.d16 = (dy<<16)/dx;
            aaedge.startP = x;
            aaedge.deltaD = dx;
            aaedge.pDst = dstImg->pData;
            aaedge.pSrc = srcImg->pData;
            aaedge.nPitch = dstImg->nPitch;
            aaedge.inc = xinc;
            aaedge.scanlinelist = scanlinelist;
            aaedge.startY = startY;
            DrawXMajorRPEdge(aaedge);
		}
		else {
            _AAEDGEINFO aaedge;
    	    aaedge.s16 = y<<16;
            aaedge.d16 = (dy<<16)/dx;
            aaedge.startP = x;
            aaedge.deltaD = dx;
            aaedge.pDst = dstImg->pData;
            aaedge.pSrc = srcImg->pData;
            aaedge.nPitch = dstImg->nPitch;
            aaedge.inc = xinc;
            aaedge.scanlinelist = scanlinelist;
            aaedge.startY = startY;
            DrawXMajorRNEdge(aaedge);
		}
	}
	else
	{
        _AAEDGEINFO aaedge;
	    aaedge.s16 = x<<16;
        aaedge.d16 = (dx<<16)/dy;
        if (xinc<0) {
           aaedge.d16=-aaedge.d16;
        }
        aaedge.startP = y;
        aaedge.deltaD = dy;
        aaedge.pDst = dstImg->pData;
        aaedge.pSrc = srcImg->pData;
        aaedge.nPitch = dstImg->nPitch;
        aaedge.inc = yinc;
        aaedge.scanlinelist = scanlinelist;
        aaedge.startY = startY;
        DrawYMajorREdge(aaedge);
	}
	return;
}



void DrawAAPolygon(POSITION * leftVertexList, int LEdgeNum, POSITION * rightVertexList, int REdgeNum, IMAGE * srcImg, IMAGE * dstImg, int level)
{
    int startY = leftVertexList[0].y;
    int height = leftVertexList[LEdgeNum].y - leftVertexList[0].y;
    ScanLineList * scanlinelist = new ScanLineList[height+2];

    int i;
    for (i=0;i<LEdgeNum;i++) {
        ScanLeftAAEdge(leftVertexList[i], leftVertexList[i+1], scanlinelist, startY, height, srcImg, dstImg);
    }
    for (i=0;i<REdgeNum;i++) {
        ScanRightAAEdge(rightVertexList[i], rightVertexList[i+1], scanlinelist, startY, height, srcImg, dstImg);
    }

    _POLYFILL pfill;

    pfill.pSrc = srcImg->pData;
    pfill.pDst = dstImg->pData;
    pfill.nSrcPitch = dstImg->nPitch;
    pfill.nDstPitch = dstImg->nPitch;
    pfill.startY = startY;
    pfill.height = height;
    pfill.scanlinelist = scanlinelist;
//    FillPolygon(scanlinelist, startY, height, srcImg, dstImg);
    FillPolygon(pfill);

    if (scanlinelist)
        delete [] scanlinelist;
}


void DrawAALine(int xs,int ys,int xe,int ye, int color, IMAGE * img)
{
	int dx=xe-xs;
	int dy=ye-ys;
	int xinc,yinc,i,x,y,pitch;

	if(dx>0) xinc=1;
	else xinc=-1;

	if(dy>0) yinc=1;
	else yinc=-1;

	dx=abs(dx);
	dy=abs(dy);
	x=xs;
	y=ys;
	i=0;

	if(dx==0&&dy==0)
		SetPixel(x, y, color, img);
	else if(dx==0) {
	    int* pDst=(int*)img->pData;
	    pitch = img->nPitch;
	    ((char*&)pDst)+=y*pitch;
	    ((char*&)pDst)+=x*4;
	    pitch*=yinc;
		for(i=0;i<dy;i++) {
		    *pDst=color;
		    ((char*&)pDst)+=pitch;
		}
	}
	else if(dy==0) {
	    int* pDst=(int*)img->pData;
	    pitch = img->nPitch;
	    ((char*&)pDst)+=y*pitch;
	    ((char*&)pDst)+=x*4;
		for(i=0;i<dx;i++) {
		    *pDst=color;
		    ((char*&)pDst)+=xinc*4;
		}
	}
	else if(dx>dy) {
	    _AALINEINFO aaline;
	    aaline.s16 = y<<16;
        aaline.d16 = (dy<<16)/dx;
        aaline.startP = x;
        aaline.deltaD = dx;
        if (yinc<0) {
            aaline.d16=-aaline.d16;
        }
        aaline.pDst = img->pData;
        aaline.nPitch = img->nPitch;
        aaline.inc = xinc;
        aaline.color = color;
        DrawXMajorLine(aaline);
	}
	else {
	    _AALINEINFO aaline;
	    aaline.s16 = x<<16;
        aaline.d16 = (dx<<16)/dy;
        aaline.startP = y;
        aaline.deltaD = dy;
        if (xinc<0) {
            aaline.d16=-aaline.d16;
        }
        aaline.pDst = img->pData;
        aaline.nPitch = img->nPitch;
        aaline.inc = yinc;
        aaline.color = color;
        DrawYMajorLine(aaline);
	}
	return;
}

void add8(short * in, short * out)
{
    out[0] = in[0]+ out[0];
    out[1] = in[1]+ out[1];
    out[2] = in[2]+ out[2];
    out[3] = in[3]+ out[3];
    return;
}

void Blit565WMMX(const ALPHABLITSTRUCT & AlphaBlitStruct)
{
    int height = AlphaBlitStruct.iDstH;
    int width = AlphaBlitStruct.iDstW;

    short * Src = (short *)AlphaBlitStruct.lpSrc;
    short * Dst = (short *)AlphaBlitStruct.lpDst;

    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            Dst[x] = Src [x];
        }
        ((char*&)Src)+=AlphaBlitStruct.iSrcPitch;
        ((char*&)Dst)+=AlphaBlitStruct.iDstPitch;
    }
}

inline unsigned char& Alpha(const IMGREGION& pic,const int x,const int y)
{
    return ( (unsigned char*)((unsigned char*)pic.paData+pic.naPitch*y) )[x];
}

inline short& Pixels(const IMGREGION& pic,const int x,const int y)
{
    return ( (short*)((unsigned char*)pic.pData+pic.nPitch*y) )[x];
}

inline short BilInear1(const IMGREGION& pic,const int x_16,const int y_16)
{
    unsigned int x=x_16>>16;
    unsigned int y=y_16>>16;
    short Color0=Pixels(pic,x,y);
    short Color2=Pixels(pic,x+1,y);
    short Color1=Pixels(pic,x,y+1);
    short Color3=Pixels(pic,x+1,y+1);

    unsigned int u_8=(x_16 & 0xFFFF)>>8;
    unsigned int v_8=(y_16 & 0xFFFF)>>8;
    unsigned int pm3_16=(u_8*v_8);
    unsigned int pm2_16=(u_8*(unsigned int)(255-v_8));
    unsigned int pm1_16=(v_8*(unsigned int)(255-u_8));
    unsigned int pm0_16=((255-u_8)*(255-v_8));

    short result = 0;

    result |=((pm0_16*(Color0&31)+pm1_16*(Color1&31)+pm2_16*(Color2&31)+pm3_16*(Color3&31))>>16);
    result |=((pm0_16*(Color0>>5&63)+pm1_16*(Color1>>5&63)+pm2_16*(Color2>>5&63)+pm3_16*(Color3>>5&63))>>16)<<5;
    result |=((pm0_16*(Color0>>11&31)+pm1_16*(Color1>>11&31)+pm2_16*(Color2>>11&31)+pm3_16*(Color3>>11&31))>>16)<<11;

    return result;
}


inline PIXEL BilInearC4A(PIXEL* PColor0, PIXEL* PColor1,unsigned long u_8,unsigned long v_8)
{
    unsigned long pm3_16=u_8*v_8;
    unsigned long pm2_16=(u_8<<8)-pm3_16;
    unsigned long pm1_16=(v_8<<8)-pm3_16;
    unsigned long pm0_16=(65535)-pm1_16-pm2_16-pm3_16;

    PIXEL result;
    //DebugBreak();
    result.A =(unsigned char)((pm0_16*PColor0[0].A+pm2_16*PColor0[1].A+pm1_16*PColor1[0].A+pm3_16*PColor1[1].A)>>16);
    result.R =(unsigned char)((pm0_16*PColor0[0].R+pm2_16*PColor0[1].R+pm1_16*PColor1[0].R+pm3_16*PColor1[1].R)>>16);
    result.G =(unsigned char)((pm0_16*PColor0[0].G+pm2_16*PColor0[1].G+pm1_16*PColor1[0].G+pm3_16*PColor1[1].G)>>16);
    result.B =(unsigned char)((pm0_16*PColor0[0].B+pm2_16*PColor0[1].B+pm1_16*PColor1[0].B+pm3_16*PColor1[1].B)>>16);

    return result;
}

inline unsigned char BilInearAlpha(const IMGREGION& pic,const int x_16,const int y_16)
{
    unsigned int x=x_16>>16;
    unsigned int y=y_16>>16;
    unsigned char Color0=Alpha(pic,x,y);
    unsigned char Color2=Alpha(pic,x+1,y);
    unsigned char Color1=Alpha(pic,x,y+1);
    unsigned char Color3=Alpha(pic,x+1,y+1);


    unsigned int u_8=(x_16 & 0xFFFF)>>8;
    unsigned int v_8=(y_16 & 0xFFFF)>>8;
    unsigned int pm3_16=(u_8*v_8);
    unsigned int pm2_16=(u_8*(unsigned int)(255-v_8));
    unsigned int pm1_16=(v_8*(unsigned int)(255-u_8));
    unsigned int pm0_16=((255-u_8)*(255-v_8));

    unsigned char result = 0;
    result = ((pm0_16*(Color0)+pm1_16*(Color1)+pm2_16*(Color2)+pm3_16*(Color3))>>16);
    return result;
}

inline unsigned char BilIneara(unsigned char* PColor0, unsigned char* PColor1,unsigned long u_8,unsigned long v_8)
{
    unsigned long pm3_16=u_8*v_8;
    unsigned long pm2_16=(u_8<<8)-pm3_16;
    unsigned long pm1_16=(v_8<<8)-pm3_16;
    unsigned long pm0_16=(65535)-pm1_16-pm2_16-pm3_16;

    unsigned char result;
    //DebugBreak();
    result  =(unsigned char)((pm0_16*PColor0[0]+pm2_16*PColor0[1]+pm1_16*PColor1[0]+pm3_16*PColor1[1])>>16);

    return result;
}

void WarpPerspective_8u_C4A(IMAGE& Src, IMAGE& Dst, int nWidth, int nLHeight, int nRHeight)
{
    _WARPPERSP para;

    para.SrcH = Src.nHeight>>1;
    para.DstW = nWidth;
    para.DstHL = nLHeight>>1;
    para.DstHR = nRHeight>>1;
    para.SrcXD16 = ((((Src.nWidth-1)<<16))/(para.DstW-1));
    para.SrcYD16 = ((((para.SrcH-1)<<16))/(para.DstHL-1));

    para.Dst_Pitch = Dst.nPitch;
    para.Src_Pitch = Src.nPitch;

    para.pSrc = (PIXEL*)(Src.pData + Src.nPitch * para.SrcH);

    if (para.DstHL >= para.DstHR) {
        para.pDst = (PIXEL*)(Dst.pData + Dst.nPitch * para.DstHL);
        para.DstHD16 = (para.DstHL<<16) - (((para.DstHR + para.DstHL * (para.DstW - 1)) <<16) / para.DstW);
        WarpPerspectiveLWMMX(para);
    }
    else {
        para.pDst = (PIXEL*)(Dst.pData + Dst.nPitch * para.DstHR);
        para.DstHD16 = (para.DstHR<<16) - (((para.DstHL + para.DstHR * (para.DstW - 1)) <<16) / para.DstW);
        WarpPerspectiveRWMMX(para);
    }

}

void WarpPerspectiveH_8u_C4A(IMAGE& Src, IMAGE& Dst, int nWidthT, int nWidthD, int nHeight)
{
    _WARPPERSPH para;
//DebugBreak();
    para.SrcW = Src.nWidth>>1;
    para.DstH = nHeight;
    para.DstWT = nWidthT>>1;
    para.DstWD = nWidthD>>1;
    para.SrcYD16 = (((Src.nHeight-1)<<16))/(para.DstH-1);
    para.SrcXD16 = (((para.SrcW-1)<<16))/(para.DstWT-1);

    para.Dst_Pitch = Dst.nPitch;
    para.Src_Pitch = Src.nPitch;

    para.pSrc = (PIXEL*)Src.pData + para.SrcW;

    if (para.DstWT >= para.DstWD) {
        para.pDst = (PIXEL*)Dst.pData + para.DstWT;
        para.DstWD16 = (para.DstWT<<16) - (((para.DstWD + para.DstWT * (para.DstH - 1)) <<16) / para.DstH);
        WarpPerspectiveTWMMX(para);
    }
    else {
        para.pDst = (PIXEL*)Dst.pData + para.DstWD;
        para.DstWD16 = (para.DstWD<<16) - (((para.DstWT + para.DstWD * (para.DstH - 1)) <<16) / para.DstH);
        WarpPerspectiveDWMMX(para);
    }

}

void AlbumTransform_8u_C4A(IMAGE& Src, IMAGE& Dst, int nWidthT, int nWidthD, int nHeight)
{
    _WARPPERSPH para;
//DebugBreak();
    para.SrcW = Src.nWidth>>1;
    para.DstH = nHeight;
    para.DstWT = nWidthT>>1;
    para.DstWD = nWidthD>>1;
    para.SrcYD16 = ((((Src.nHeight-1)<<16))/(para.DstH-1));
    para.SrcXD16 = ((((para.SrcW-1)<<16))/(para.DstWT-1));

    para.Dst_Pitch = Dst.nPitch;
    para.Src_Pitch = Src.nPitch;

    para.pSrc = (PIXEL*)Src.pData + para.SrcW;

    if (para.DstWT >= para.DstWD) {
        para.pDst = (PIXEL*)Dst.pData + 3 * para.DstWT;
        para.DstWD16 = (para.DstWT<<16) - (((para.DstWD + para.DstWT * (para.DstH - 1)) <<16) / para.DstH);
        WarpPerspectiveTWMMX(para);
    }
    else {
        para.pDst = (PIXEL*)Dst.pData + 3 * para.DstWD;
        para.DstWD16 = (para.DstWD<<16) - (((para.DstWT + para.DstWD * (para.DstH - 1)) <<16) / para.DstH);
        WarpPerspectiveDWMMX(para);
    }

}

void ShearBilinear_8u_C4A(IMAGE& Src, IMAGE& Dst, int offset)
{
    if ((abs(offset)+1)>Dst.nWidth)
        return;

    _SHEAR shear;

    shear.pSrc = (PIXEL*)Src.pData;
    shear.pDst = (PIXEL*)Dst.pData;

    shear.SrcW = Src.nWidth;
    shear.SrcH = Src.nHeight;
    shear.DstW = Dst.nWidth - abs(offset);
    shear.DstH =  Dst.nHeight;

    shear.SrcXD16 = ((((shear.SrcW-1)<<16))/(shear.DstW-1));
    shear.SrcYD16 = ((((shear.SrcH-1)<<16))/(shear.DstH-1));
    shear.DstSX16 = (abs(offset)<<16)/shear.DstH;

    shear.Dst_Pitch = Dst.nPitch;
    shear.Src_Pitch = Src.nPitch;
    shear.offset = abs(offset);

    if (offset>=0) {
        shear.Dst_SX16 = 0;
        ShearBilinearP_8u_C4A(shear);

    }
    else {
        shear.Dst_SX16 = abs(offset)<<16;
        ShearBilinearM_8u_C4A(shear);
    }
}

void CurveBilinear_8u_C4A(IMAGE& Src, IMAGE& Dst, int* aSampleX16, int width)
{

    _CURVE curve;

    curve.pSrc = (PIXEL*)Src.pData;
    curve.pDst = (PIXEL*)Dst.pData;

    curve.SrcW = Src.nWidth;
    curve.SrcH = Src.nHeight;
    curve.DstW = Dst.nWidth;
    curve.DstH = Dst.nHeight;

    curve.SrcXD16 = (((curve.SrcW-1)<<16))/(curve.DstW)+1;
    curve.SrcYD16 = (((curve.SrcH-1)<<16))/(curve.DstH)+1;
    curve.aSampleX16 = aSampleX16;
    curve.Dst_SX16 = aSampleX16[0];

    curve.Dst_Pitch = Dst.nPitch;
    curve.Src_Pitch = Src.nPitch;
    curve.InnerWidth = width;

    CurveBilinearWMMX_8u_C4A(curve);
}

void AdditiveCurveBilinearV_8u_C4A(IMAGE& Src, IMAGE& Dst, int* aSampleX16, int offset, int width)
{
    _CURVE curve;

    curve.pSrc = (PIXEL*)Src.pData;
    curve.pDst = (PIXEL*)(Dst.pData+(offset<<2));

    curve.SrcW = Src.nWidth;
    curve.SrcH = Src.nHeight;
    curve.DstW = Dst.nWidth;
    curve.DstH = Dst.nHeight;

    curve.SrcXD16 = (((curve.SrcW-1)<<16))/(width)+1;
    curve.SrcYD16 = (((curve.SrcH-1)<<16))/(curve.DstH)+1;
    curve.aSampleX16 = aSampleX16;
    curve.Dst_SX16 = aSampleX16[0];

    curve.Dst_Pitch = Dst.nPitch;
    curve.Src_Pitch = Src.nPitch;
    curve.InnerWidth = width;

//    AdditiveCurveBilinearWMMXV_8u_C4A(curve);
}
//
//void RGB888To565(IMAGE &image, SURFACE &surface, INVALIDREGION &rect, int orientation)
//{
//    IMAGE Src = image;
//    SURFACE Dst = surface;
//
//    switch (orientation) {
//        case 0:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += (rect.tx+rect.ty*surface.nWidth);
//            Dst.nPitch = surface.nPitch;
//            RECT888To565(&Src,&Dst);
//            break;
//        case 1:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += (surface.nWidth-rect.by)+rect.tx*surface.nWidth;
//            Dst.nPitch = surface.nPitch;
//            RECT888To565RCW(&Src,&Dst);
//            break;
//        case 2:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += (surface.nWidth-rect.tx)+(surface.nHeight-rect.ty)*surface.nWidth;
//            Dst.nPitch = surface.nPitch;
//            RECT888To565FLIP(&Src, &Dst);
//            break;
//        case 3:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += rect.ty + (surface.nHeight-rect.tx)*surface.nWidth;
//            Dst.nPitch = surface.nPitch;
//            RECT888To565RCCW(&Src,&Dst);
//            break;
//    }
//}


//void RECT888To666(IMAGE * image, IMAGE * surface)
//{
//    int x, y, width, height;
//    width = image->nWidth;
//    height = image->nHeight;
//    PIXEL* Src = (PIXEL*)image->pData;
//    int * Dst = (int*)surface->pData;
//    int temp;
//    for (y=0;y<height;y++) {
//        for (x=0;x<width;x++) {
//            temp = Src[x].B>>2;
//            temp |= (Src[x].G>>2)<<6;
//            temp |= (Src[x].R>>2)<<12;
//            Dst[x] = temp;
//        }
//        ((char*&)Src)+=image->nPitch;
//        ((char*&)Dst)+=surface->nPitch;
//    }
//}
//
//void RECT888To666RCW(IMAGE * image, IMAGE * surface)
//{
//    int x, y, width, height;
//    width = image->nWidth;
//    height = image->nHeight;
//    PIXEL* Src = (PIXEL*)image->pData;
//    ((char*&)Src)+=image->nPitch*(image->nHeight-1);
//    int * Dst = (int*)surface->pData;
//    int * DstLine = Dst;
//    int temp;
//    for (y=0;y<height;y++) {
//        for (x=0;x<width;x++) {
//            temp = Src[x].B>>2;
//            temp |= (Src[x].G>>2)<<6;
//            temp |= (Src[x].R>>2)<<12;
//            *Dst = temp;
//            ((char*&)Dst)+=surface->nPitch;
//        }
//        ((char*&)Src)-=image->nPitch;
//        DstLine+=1;
//        Dst = DstLine;
//    }
//}
//
//void RECT888To666RCCW(IMAGE * image, IMAGE * surface)
//{
//    int x, y, width, height;
//    width = image->nWidth;
//    height = image->nHeight;
//    PIXEL* Src = (PIXEL*)image->pData;
//    int * Dst = (int*)surface->pData;
//    int * DstLine = Dst;
//    int temp;
//    for (y=0;y<height;y++) {
//        for (x=0;x<width;x++) {
//            temp = Src[x].B>>2;
//            temp |= (Src[x].G>>2)<<6;
//            temp |= (Src[x].R>>2)<<12;
//            *Dst = temp;
//            ((char*&)Dst)-=surface->nPitch;
//        }
//        ((char*&)Src)+=image->nPitch;
//        DstLine+=1;
//        Dst = DstLine;
//    }
//}

//void RECT888To666FLIP(IMAGE * image, IMAGE * surface)
//{
//    int x, y, width, height;
//    width = image->nWidth;
//    height = image->nHeight;
//    PIXEL* Src = (PIXEL*)image->pData;
//    int * Dst = (int*)surface->pData;
//    int * DstLine = Dst;
//    int temp;
//    for (y=0;y<height;y++) {
//        for (x=0;x<width;x++) {
//            temp = Src[x].B>>2;
//            temp |= (Src[x].G>>2)<<6;
//            temp |= (Src[x].R>>2)<<12;
//            *--Dst = temp;
//        }
//        ((char*&)Src)+=image->nPitch;
//        ((char*&)DstLine)-=surface->nPitch;
//        Dst = DstLine;
//    }
//}

//
//void RGB888To666(IMAGE &image, IMAGE &surface, INVALIDREGION &rect, int orientation)
//{
//    IMAGE Src = image;
//    IMAGE Dst = surface;
//
//    switch (orientation) {
//        case 0:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += (rect.tx+rect.ty*surface.nWidth)<<2;
//            Dst.nPitch = surface.nPitch;
//            RECT888To666(&Src,&Dst);
//            break;
//        case 1:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += ((surface.nWidth-rect.by)+rect.tx*surface.nWidth)<<2;
//            Dst.nPitch = surface.nPitch;
//            RECT888To666RCW(&Src,&Dst);
//            break;
//        case 2:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += ((surface.nWidth-rect.tx)+(surface.nHeight-rect.ty)*surface.nWidth)<<2;
//            Dst.nPitch = surface.nPitch;
//            RECT888To666FLIP(&Src, &Dst);
//            break;
//        case 3:
//            Src.pData += (rect.tx+rect.ty*image.nWidth)<<2;
//            Src.nWidth = rect.bx - rect.tx;
//            Src.nHeight = rect.by - rect.ty;
//            Dst.pData += (rect.ty + (surface.nHeight-rect.tx)*surface.nWidth)<<2;
//            Dst.nPitch = surface.nPitch;
//            RECT888To666RCCW(&Src,&Dst);
//            break;
//    }
//}


void Dither_ErrorDiffuse_Line(PIXEL* pDst, PIXEL* pSrc,int width,_ErrorColor* PHLineErr, int Rbit, int Gbit, int Bbit)
{
    _ErrorColor HErr;

    int Bfactor = Power2m1(Bbit);
    int Gfactor = Power2m1(Gbit);
    int Rfactor = Power2m1(Rbit);

    HErr.dR=0; HErr.dG=0; HErr.dB=0;

    PHLineErr[-1].dB=0; PHLineErr[-1].dG=0; PHLineErr[-1].dR=0;

    for (long x=0;x<width;++x)
    {

        float cB=(pSrc[x].B+HErr.dB*2+PHLineErr[x].dB+PHLineErr[x-1].dB);
        float cG=(pSrc[x].G+HErr.dG*2+PHLineErr[x].dG+PHLineErr[x-1].dG);
        float cR=(pSrc[x].R+HErr.dR*2+PHLineErr[x].dR+PHLineErr[x-1].dR);

        int rB=getBestColor(cB, Bfactor);
        int rG=getBestColor(cG, Gfactor);
        int rR=getBestColor(cR, Rfactor);

        pDst[x].R = (unsigned char)rR;
        pDst[x].G = (unsigned char)rG;
        pDst[x].B = (unsigned char)rB;

        HErr.dB=float((cB-(rB*(255.0/Bfactor)))*(1.0/4));
        HErr.dG=float((cG-(rG*(255.0/Gfactor)))*(1.0/4));
        HErr.dR=float((cR-(rR*(255.0/Rfactor)))*(1.0/4));

        PHLineErr[x-1].dB+=HErr.dB;
        PHLineErr[x-1].dG+=HErr.dG;
        PHLineErr[x-1].dR+=HErr.dR;

        PHLineErr[x]=HErr;
    }
}

void Dither_ErrorDiffuse(IMAGE& dst,const IMAGE& src, int Rbit, int Gbit, int Bbit)
{
    PIXEL* pDst=(PIXEL*)dst.pData;
    PIXEL* pSrc=(PIXEL*)src.pData;
    int width=src.nWidth;

    _ErrorColor* _HLineErr=new _ErrorColor[width+2];

    for (long x=0;x<width+2;++x){
        _HLineErr[x].dR=0;
        _HLineErr[x].dG=0;
        _HLineErr[x].dB=0;
    }

    _ErrorColor* HLineErr=&_HLineErr[1];

    for (long y=0;y<src.nHeight;++y){
        Dither_ErrorDiffuse_Line(pDst,pSrc,width,HLineErr,Rbit,Gbit,Bbit);
        (unsigned char*&)pDst+=dst.nPitch;
        (unsigned char*&)pSrc+=src.nPitch;
    }

    delete[]_HLineErr;
}


void DitherImage(unsigned char * pData, int width, int height, int Rbit, int Gbit, int Bbit)
{
    IMAGE Src;
    IMAGE Dst;

    Src.pData = pData;
    Src.nWidth = width;
    Src.nHeight = height;
    Src.nPitch = width*4;

    Dst.pData = (unsigned char*)malloc(height * width * 4);
    Dst.nPitch = Src.nPitch;
    Dst.nWidth = Src.nWidth;
    Dst.nHeight = Src.nHeight;

    Dither_ErrorDiffuse(Dst,Src,Rbit,Gbit,Bbit);

    PIXEL* src = (PIXEL*)Src.pData;
    PIXEL* dst = (PIXEL*)Dst.pData;

    for (int i=0;i<width*height;i++) {
        (*src).R = (*dst).R<<(8-Rbit);
        (*src).G = (*dst).G<<(8-Gbit);
        (*src).B = (*dst).B<<(8-Bbit);
        dst++;
        src++;
    }

    if (Dst.pData) delete Dst.pData;
}
